#include <errno.h>
#include "binary.h"

int 
writebytes(Writer *writer, u_int8_t *data, size_t count)
{
	if (writer->offset + count > writer->size) {
		writer->buffer = (u_int8_t*) realloc(writer->buffer, writer->offset + count);
		if (writer->buffer == NULL) {
			writer->offset = 0;
			writer->size = 0;
			return -1;
		}
		writer->size = writer->offset + count;
	}
	for (size_t i = 0; i < count; ++i)
		writer->buffer[writer->offset++] = data[i];
	return 0;
}

int 
writebyte(Writer *writer, u_int8_t data)
{
	return writebytes(writer, &data, 1);
}

int
writeshort(Writer *writer, enum Endian endian, u_int16_t data)
{
	u_int8_t temp[2] = { 0, 0 };
	if (endian == Big) {
		temp[0] = (data >> 8) & 0xff;
		temp[1] = data & 0xff;
	} else {
		temp[0] = data & 0xff;
		temp[1] = (data >> 8) & 0xff;
	}
	return writebytes(writer, temp, 2);
}

int
writetriad(Writer *writer, enum Endian endian, u_int32_t data)
{
	u_int8_t temp[3] = { 0, 0, 0 };
	if (endian == Big) {
		temp[0] = (data >> 16) & 0xff;
		temp[1] = (data >> 8) & 0xff;
		temp[2] = data & 0xff;
	} else {
		temp[0] = data & 0xff;
		temp[1] = (data >> 8) & 0xff;
		temp[2] = (data >> 16) & 0xff;
	}
	return writebytes(writer, temp, 3);
}

int
writeint(Writer *writer, enum Endian endian, u_int32_t data)
{
	u_int8_t temp[4] = { 0, 0, 0, 0 };
	if (endian == Big) {
		temp[0] = (data >> 24) & 0xff;
		temp[1] = (data >> 16) & 0xff;
		temp[2] = (data >> 8) & 0xff;
		temp[3] = data & 0xff;
	} else {
		temp[0] = data & 0xff;
		temp[1] = (data >> 8) & 0xff;
		temp[2] = (data >> 16) & 0xff;
		temp[3] = (data >> 24) & 0xff;
	}
	return writebytes(writer, temp, 4);
}

int
writevarint(Writer *writer, u_int32_t data)
{
	data &= 0xffffffff;
	u_int8_t temp = 0;
	for (u_int8_t i = 0; i < 5; ++i) {
		if ((data >> 7) != 0) { 
			temp = data | 0x80;
			writebytes(writer, &temp, 1);
		} else {
			temp = data & 0x7f;
			writebytes(writer, &temp, 1);
			return 0;
		}
		data >>= 7;
	}
	return -1;
}

int
writelong(Writer *writer, enum Endian endian, u_int64_t data)
{
	u_int8_t temp[8] = { 0, 0, 0, 0, 0, 0, 0, 0 };
	if (endian == Big) {
		temp[0] = (data >> 56) & 0xff;
		temp[1] = (data >> 48) & 0xff;
		temp[2] = (data >> 40) & 0xff;
		temp[3] = (data >> 32) & 0xff;
		temp[4] = (data >> 24) & 0xff;
		temp[5] = (data >> 16) & 0xff;
		temp[6] = (data >> 8) & 0xff;
		temp[7] = data & 0xff;
	} else {
		temp[0] = data & 0xff;
		temp[1] = (data >> 8) & 0xff;
		temp[2] = (data >> 16) & 0xff;
		temp[3] = (data >> 24) & 0xff;
		temp[4] = (data >> 32) & 0xff;
		temp[5] = (data >> 40) & 0xff;
		temp[6] = (data >> 48) & 0xff;
		temp[7] = (data >> 56) & 0xff;
	}
	return writebytes(writer, temp, 8);
}

int
writevarlong(Writer *writer, u_int64_t data)
{
	data &= 0xffffffff;
	u_int8_t temp = 0;
	for (u_int8_t i = 0; i < 10; ++i) {
		if ((data >> 7) != 0) { 
			temp = data | 0x80;
			writebytes(writer, &temp, 1);
		} else {
			temp = data & 0x7f;
			writebytes(writer, &temp, 1);
			return 0;
		}
		data >>= 7;
	}
	return -1;
}

int
writeaddressv4(Writer *writer, struct sockaddr_in address)
{
	if (
		(writebyte(writer, 4) != 0) 
		|| (writebytes(writer, (u_int8_t *)&address.sin_addr.s_addr, 4) != 0) 
		|| (writeshort(writer, Big, htons(address.sin_port)) != 0)
	)
		return -1;
	return 0;
}

int
readbytes(Reader *reader, u_int8_t *data, size_t count) 
{
	if (reader->offset + count > reader->size) {
		errno = ENOBUFS;
		return -1;
	}
	for (size_t i = 0; i < count; ++i)
		data[i] = reader->buffer[reader->offset++];
	return 0;
}

int
readbyte(Reader *reader, u_int8_t *data)
{
	return readbytes(reader, data, 1);
}

int
readshort(Reader *reader, u_int16_t *data, enum Endian endian)
{
	u_int8_t input[2] = { 0, 0};
	if (readbytes(reader, input, 2) != 0)
		return -1;
	if (endian == Big)
		*data = ((u_int16_t)(input[0] & 0xff) << 8)
			| (u_int16_t)(input[1] & 0xff);
	else
		*data = (u_int16_t)(input[0] & 0xff)
			| ((u_int16_t)(input[1] & 0xff) << 8);
	return 0;
}

int
readtriad(Reader *reader, u_int32_t *data, enum Endian endian)
{
	u_int8_t input[3] = { 0, 0, 0};
	if (readbytes(reader, input, 3) != 0)
		return -1;
	if (endian == Big)
		*data = ((u_int32_t)(input[0] & 0xff) << 16)
			| ((u_int32_t)(input[1] & 0xff) << 8)
			| (u_int32_t)(input[2] & 0xff);
	else
		*data = (u_int32_t)(input[0] & 0xff)
			| ((u_int32_t)(input[1] & 0xff) << 8)
			| ((u_int32_t)(input[2] & 0xff) << 16);
	return 0;
}

int
readint(Reader *reader, u_int32_t *data, enum Endian endian)
{
	u_int8_t input[4] = { 0, 0, 0, 0 };
	if(readbytes(reader, input, 4) != 0)
		return -1;
	if (endian == Big)
		*data = ((u_int32_t)(input[0] & 0xff) << 24)
			| ((u_int32_t)(input[1] & 0xff) << 16)
			| ((u_int32_t)(input[2] & 0xff) << 8)
			| (u_int32_t)(input[3] & 0xff);
	else
		*data = (u_int32_t)(input[0] & 0xff)
			| ((u_int32_t)(input[1] & 0xff) << 8)
			| ((u_int32_t)(input[2] & 0xff) << 16)
			| ((u_int32_t)(input[3] & 0xff) << 24);
	return 0;
}

int
readvarint(Reader *reader, u_int32_t *data)
{
	u_int8_t temp = 0;
	for (size_t i = 0; i < 28; ++i) {
		readbytes(reader, &temp, 1);
		*data |= ((u_int32_t)(temp & 0x7f)) << ((u_int32_t)i);
		if ((temp & 0x80) == 0)
			return 0;
	}
	return -1;
}

int
readlong(Reader *reader, u_int64_t *data, enum Endian endian)
{
	u_int8_t input[8] = { 0, 0, 0, 0, 0, 0, 0, 0 };
	if (readbytes(reader, input, 8) != 0)
		return -1;
	if (endian == Big)
		*data = ((u_int64_t)(input[0] & 0xff) << 56)
			| ((u_int64_t)(input[1] & 0xff) << 48)
			| ((u_int64_t)(input[2] & 0xff) << 40)
			| ((u_int64_t)(input[3] & 0xff) << 32)
			| ((u_int64_t)(input[4] & 0xff) << 24)
			| ((u_int64_t)(input[5] & 0xff) << 16)
			| ((u_int64_t)(input[6] & 0xff) << 8)
			| (u_int64_t)(input[7] & 0xff);
	else
		*data = (u_int64_t)(input[0] & 0xff)
			| ((u_int64_t)(input[1] & 0xff) << 8)
			| ((u_int64_t)(input[2] & 0xff) << 16)
			| ((u_int64_t)(input[3] & 0xff) << 24)
			| ((u_int64_t)(input[4] & 0xff) << 32)
			| ((u_int64_t)(input[5] & 0xff) << 40)
			| ((u_int64_t)(input[6] & 0xff) << 48)
			| ((u_int64_t)(input[7] & 0xff) << 56);
	return 0;
}

int
readvarlong(Reader *reader, u_int64_t *data)
{
	u_int8_t temp = 0;
	for (size_t i = 0; i < 63; ++i) {
		if (readbyte(reader, &temp) != 0)
			return -1;
		*data |= ((u_int64_t)(temp & 0x7f)) << ((u_int64_t)i);
		if ((temp & 0x80) == 0)
			return 0;
	}
	return -1;
}

int
readaddressv4(Reader *reader, struct sockaddr_in *addr)
{
	u_int8_t temp;
	if (readbyte(reader, &temp) != 0)
		return -1;
	addr->sin_family = AF_INET;
	if (readbytes(reader, (u_int8_t *)&addr->sin_addr.s_addr, 4) != 0)
		return -1;
	u_int16_t port = 0;
	if (readshort(reader, &port, Big) != 0)
		return -1;
	addr->sin_port = htons(port);
	return 0;
}
